package it.gius.processing.gjkTest;


import org.jbox2d.common.Vec2;

import processing.core.PApplet;
import it.gius.pe.Shape.Polygon;
import it.gius.pe.algorithm.gjk.GjkUtilsFunctions;
import it.gius.pe.algorithm.gjk.Simplex;
import it.gius.pe.algorithm.gjk.SimplexSolution;
import it.gius.pe.mains.MainMovingTest;
import it.gius.processing.util.MyAbstractPApplet;

public class GjkPolyPointStepToStepApplet extends MyAbstractPApplet {


	private Polygon poly = null;
	private Vec2 q = null;
	private GjkUtilsFunctions utils = null;
	
	private static float EPSILON = 1E-3f; 
	
	private Simplex simplex = null;
	private SimplexSolution sol = null;

	public GjkPolyPointStepToStepApplet(Polygon poly,Vec2 q) {
		this.poly = poly;
		this.q = q;
		utils = new GjkUtilsFunctions();
		sol = new SimplexSolution();
	}

	
	public void setup(){

		size(MainMovingTest.sizeX,MainMovingTest.sizeY);
		smooth();
		
		simplex = new Simplex(Simplex.LARGER_2D_SIMPLEX);
		simplex.vs[0].set(poly.getVertex(0));
		simplex.currentDim = Simplex.IS_0_SIMPLEX;
		

		
		background(255);

		fill(0,0,210);
		stroke(10);
		drawPoly(poly);
		
		fill(200,0,0);
		ellipse(q.x,q.y,3,3);
		
		noLoop();

	}


	
	@Override
	public void keyPressed() {
		super.keyPressed();
		
		if(key == 'a')
		{
			redraw();
		}
	}

	
	private boolean done = false;
	Vec2 currSol = new Vec2();
	public void draw()
	{
		
		if(done)
			return;
		
		println("Draw called");
		utils.nearestPointSimplex2D(simplex, q, sol);
		
		sol.getSolutionPoint(currSol);
		
		float g = utils.g(currSol,q,poly);
		println("g: " + g);
		if(g >= -EPSILON && g<= EPSILON)
		{
			//text("DONE");
			println("DONE");
			done = true;
		}

		background(255);
		fill(0,0,210);
		stroke(10);
		drawPoly(poly);

		drawSimplex(simplex);
		
		fill(200,0,0);
		ellipse(q.x,q.y,3,3);
		
		sol.getSolutionPoint(currSol);
		
		fill(0,200,0);
		ellipse(currSol.x,currSol.y,3,3);
		
		//current mid distance
		stroke(200,200,0);
		line(currSol.x,currSol.y,q.x,q.y);
		
		if(sol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
		{
			println("Usati max vertici: punto interno a poligono");
			//text("punto interno");
		}
		
		int index = utils.supportPointIndex(poly, q.sub(currSol));
		
		boolean supportAlreadyIn = false;
		
		for(int i =0; i< sol.numVerticesUsed;i++)
		{
			simplex.vs[i] = sol.verticesInSolution[i];
			if(simplex.vs[i].equals(poly.getVertex(index)))
			{
				println("adding a vertex already in simplex");
				//text("adding a vertex already in simplex");
				supportAlreadyIn = true;
			}
		}
		
		simplex.currentDim = sol.numVerticesUsed;
		
		if(supportAlreadyIn)
			return;
		
		if(simplex.currentDim < Simplex.LARGER_2D_SIMPLEX)
		{
			simplex.vs[sol.numVerticesUsed] = poly.getVertex(index);
			simplex.currentDim++;
		}
		
	}
	
	private void drawSimplex(Simplex simplex)
	{
		
		if(simplex.currentDim == Simplex.IS_0_SIMPLEX)
		{
			fill(220,0,0);
			ellipse(simplex.vs[0].x,simplex.vs[0].y,5,5);
			return;
		}
		
		noFill();
		stroke(220,0,0);
		beginShape();
		for(int i = 0; i < simplex.currentDim; i++)
		{
			vertex(simplex.vs[i].x,simplex.vs[i].y);
		}
		endShape(PApplet.CLOSE);
	}

	private void drawPoly(Polygon poly)
	{
		beginShape();
		for(int i=0; i< poly.getVerticesCount(); i++)
		{
			Vec2 v = poly.getVertex(i);
			vertex(v.x,v.y);
		}
		endShape(PApplet.CLOSE);

	}
}
